
#include <sys/types.h>
#include <dirent.h>
#include <string.h>
#include <stddef.h>
#include <vector>
#include <iostream>
#include <fstream>
#include <array>
#include <functional>
#include <algorithm>
#include <regex>

#include "local_data.h"

#ifdef __ANDROID__
#include <android/log.h>
#define LOG_TAG    "controller_alg.cpp"
#define DPRINTF(...)  __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__)
#define IPRINTF(...)  __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
#define EPRINTF(...)  __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
#else
#define DPRINTF(...)  printf(__VA_ARGS__)
#define IPRINTF(...)  printf(__VA_ARGS__)
#define EPRINTF(...)  printf(__VA_ARGS__)
#endif


static char *strstrip(char *s)
{
    size_t size;
    char *end;

    size = strlen(s);

    if (!size)
        return s;

    end = s + size - 1;
    while (end >= s && isspace(*end))
        end--;
    *(end + 1) = '\0';

    while (*s && isspace(*s))
        s++;

    return s;
}



static void get_image_file_list_(const char *root_dir,
                                 std::vector<std::tuple<uint64_t, std::string, std::string>> &images
)
{
    DIR *dp;
    struct dirent *ep;
    dp = opendir (root_dir);
    if (dp == NULL) {
        EPRINTF("Couldn't open the directory: %s\n", root_dir);
        return;
    }
    uint64_t start_ts = ~0;
    while ((ep = readdir (dp))) {
        if (ep->d_name[0] == '.' ||
            strcasecmp(ep->d_name, "sensorfile.txt") == 0 ||
            strcasecmp(ep->d_name, "imgfile.txt") == 0 ||
            strchr(ep->d_name, '.')) {
            continue;
        }
//        IPRINTF("%s\n", ep->d_name);
        char *cp = strdup(ep->d_name);
        char *seq = strtok(cp, "_"); // seq
        char *ts = strtok(NULL, "_"); // ts
        uint64_t t = atoll(ts);
//        if (t < start_ts) {
//            start_ts = t;
//        }
//        IPRINTF("%s    %ld\n", ts, t);
        char *fn = strtok(NULL, "_"); // fn
        char *type = strtok(NULL, "_"); // type
        //IPRINTF("%s\n", type);
        if (strstr(ep->d_name, "a") != nullptr) {
            images.push_back(std::tuple<uint64_t, std::string, std::string>(t, ep->d_name, "a"));
        } else if (strstr(ep->d_name, "b") != nullptr) {
            images.push_back(std::tuple<uint64_t, std::string, std::string>(t, ep->d_name, "b"));
        } else {
            EPRINTF("bad image type: %s\n", type);
        }

        free(cp);
    }
    (void) closedir (dp);
}


static void get_images_files_list(const char *root_dir, std::vector<std::tuple<uint64_t, std::string, std::string>> &images)
{
    images.clear();

    get_image_file_list_(root_dir, images);
    IPRINTF("images number: %lu\n", images.size());

    auto cmp = std::bind([](const std::tuple<uint64_t, std::string, std::string> &left, const std::tuple<uint64_t, std::string, std::string> &right){
        return std::get<0>(left) < std::get<0>(right);
    }, std::placeholders::_1, std::placeholders::_2);
    std::sort(images.begin(), images.end(), cmp);
}

void get_sensor_data(const char *root_dir, std::vector<std::tuple<uint64_t, int, std::array<double, 12>>> &sensor_data)
{
    char path[512] = {0};
    sprintf(path, "%s/sensorfile.txt", root_dir);
    std::ifstream fin(path);
    char line[1024];
    uint64_t prev_ts = 0;
    while (fin.getline(line, sizeof(line))) {
        //IPRINTF("%s\n", line);
        char *type_s = strstrip(strtok(line, ":"));
        char *data_s = strtok(NULL, ":");
        if (strcmp(type_s, "left") == 0) {
            uint64_t ts, gts, ats;
            double gx, gy, gz, ax, ay, az;
            sscanf(data_s, "%ld %ld %lf %lf %lf %ld %lf %lf %lf", &ts, &gts, &gx, &gy, &gz, &ats, &ax, &ay, &az);

            std::array<double, 12> data;
            data[0] = gx; data[1] = gy, data[2] = gz;
            data[3] = ax; data[4] = ay, data[5] = az;
            sensor_data.push_back(std::make_tuple(ts, 0, data));
        } else if (strcmp(type_s, "right") == 0) {
            uint64_t ts, gts, ats;
            double gx, gy, gz, ax, ay, az;
            sscanf(data_s, "%ld %ld %lf %lf %lf %ld %lf %lf %lf", &ts, &gts, &gx, &gy, &gz, &ats, &ax, &ay, &az);

            std::array<double, 12> data;
            data[0] = gx; data[1] = gy, data[2] = gz;
            data[3] = ax; data[4] = ay, data[5] = az;
            sensor_data.push_back(std::make_tuple(ts, 1, data));
        } else if (strcmp(type_s, "head pose") == 0) {
            uint64_t ts;
            std::array<double, 12> data;
            double tmp;
            sscanf(data_s, "%ld %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", &ts,
                   &data[0], &data[1], &data[2], &tmp,
                   &data[3], &data[4], &data[5], &tmp,
                   &data[6], &data[7], &data[8], &tmp,
                   &data[9], &data[10], &data[11], &tmp);
            sensor_data.push_back(std::make_tuple(ts, 2, data));
        } else {
            continue;
        }

    }
	
	auto cmp = std::bind([](const std::tuple<uint64_t, int, std::array<double, 12>> &left, const std::tuple<uint64_t, int, std::array<double, 12>> &right){
        return std::get<0>(left) < std::get<0>(right);
    }, std::placeholders::_1, std::placeholders::_2);
    std::sort(sensor_data.begin(), sensor_data.end(), cmp);
}


void get_dataset(const char *root_dir, std::vector<DataMeta> &dataset)
{
    std::vector<std::tuple<uint64_t, std::string, std::string>> images;

    std::vector<std::tuple<uint64_t, int, std::array<double, 12>>> sensor_data;

    dataset.clear();

    DIR *dir = opendir(root_dir);
    if (dir == NULL) {
        EPRINTF("open dir %s failed\n", root_dir);
        return;
    }
    closedir(dir);

    get_sensor_data(root_dir, sensor_data);
    IPRINTF("sensor data number: %lu\n", sensor_data.size());

    get_images_files_list(root_dir, images);
    IPRINTF("images number: %lu\n", images.size());
    int j = 0;
    for (int i = 0; i < images.size(); i++) {
        auto ts_image = std::get<0>(images[i]);
        for (; j < sensor_data.size(); j++) {
            auto ts_imu = std::get<0>(sensor_data[j]);
            if (ts_imu <= ts_image) {
                dataset.push_back(std::move(DataMeta(DataMeta::DATA_SENSOR, sensor_data[j])));
            } else {
                break;
            }
        }
        dataset.push_back(DataMeta(DataMeta::DATA_IMAGE, images[i]));
    }
    IPRINTF("dataset number: %lu\n", dataset.size());
}
